# 全局开关
import re
import time

output_mod = True


class Control:
    connect = None

    def __init__(self, connection=None, cmd_q=None):
        self.connect = connection
        self.cmd_q = cmd_q

    def wait(self):
        if output_mod:
            print(" *C* wait:", self.connect.rec_result())
        else:
            self.connect.rec_result()

    # 返回前方距离,单位mm
    def f_distance(self):
        while True:
            self.connect.send_ESP32(cmd="tof?")
            if output_mod:
                print(" *C* ask: tof?")
            s = self.connect.rec_result()
            d = re.findall("\d+\.?\d*", s)
            d = list(map(int, d))[0]
            if output_mod:
                print(" *C* distance:", d)
            if s[0] == 'u' or d < 100:
                time.sleep(0.2)
            else:
                break
        return d

    def get_speed(self):  # 获取当前设置速度（cm/s）
        self.connect.send(cmd='speed?')
        if output_mod:
            print(" *C* ask:", 'speed?')
        speed = int(self.connect.rec_result())
        if output_mod:
            print(" *C* speed:", speed)
        return speed

    def get_battery(self):  # 获取当前电池剩余电量的百分比值
        self.connect.send(cmd='battery?')
        if output_mod:
            print(" *C* ask:", 'battery?')
        battery = int(self.connect.rec_result())
        if output_mod:
            print(" *C* battery level(%):", battery)
        return battery

    def get_time(self):  # 获取电机运转时间（s）
        self.connect.send(cmd='time?')
        if output_mod:
            print(" *C* ask:", 'time?')
        tello_time = int(self.connect.rec_result())
        if output_mod:
            print(" *C* time:", tello_time)
        return tello_time

    def get_wifi(self):  # 获得 Wi-Fi 信噪比
        self.connect.send(cmd='wifi?')
        if output_mod:
            print(" *C* ask:", 'wifi?')
        wifi = int(self.connect.rec_result())
        if output_mod:
            print(" *C* wifi:", wifi)
        return wifi

    def get_sdk(self):  # 获得 Tello SDK 版本号
        self.connect.send(cmd='sdk?')
        if output_mod:
            print(" *C* ask:", 'sdk?')
        wifi = self.connect.rec_result()
        if output_mod:
            print(" *C* wifi:", wifi)
        return wifi

    def get_sn(self):  # 获得 Tello SN 码
        self.connect.send(cmd='sn?')
        if output_mod:
            print(" *C* ask:", 'sn?')
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* sn:", rec)
        return rec

    def get_hardware(self):  # 获得 TT 是否接入开源控制器，接入返回 RMTT，没接则返回 TELLO
        self.connect.send(cmd='hardware?')
        if output_mod:
            print(" *C* ask:", 'hardware?')
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* hardware:", rec)
        return rec

    def get_wifiversion(self):  # 查询开源控制器 -WiFi 版本（仅开源控制器支持）
        self.connect.send(cmd='wifiversion?')
        if output_mod:
            print(" *C* ask:", 'wifiversion?')
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* wifiversion:", rec)
        return rec

    def get_ap(self):  # 获取当前待连接的路由器名称和密码（仅开源控制器支持）
        self.connect.send(cmd='ap?')
        if output_mod:
            print(" *C* ask:", 'ap?')
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* ap:", rec)
        return rec

    def get_ssid(self):  # 获取飞行器当前的 SSID（仅开源控制器支持）
        self.connect.send(cmd='ssid?')
        if output_mod:
            print(" *C* ask:", 'ssid?')
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* ssid:", rec)
        return rec

    def muti_wifi(self, wifi_id, wifi_password):  # 设置开源控制器 SSID 与密码，支持连接多台设备作为路由器使用
        self.connect.send(cmd='mutiwifi {} {}'.format(wifi_id, wifi_password))
        if output_mod:
            print(" *C* set:", 'mutiwifi {} {}'.format(wifi_id, wifi_password))
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* mutiwifi:", rec)
        return rec

    def set_vision(self, setting):  # 将当前速度设为 x cm/s
        """
        :param setting: 0为前 1为下
        :return:
        """
        self.connect.send(cmd='downvision {}'.format(setting))
        if output_mod:
            print(" *C* set:", 'downvision {}'.format(setting))
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* downvision:", rec)
        return rec

    def set_speed(self, speed):  # 将当前速度设为 x cm/s
        self.connect.send(cmd='speed {}'.format(speed))
        if output_mod:
            print(" *C* set:", 'speed {}'.format(speed))
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* speed:", rec)
        return rec

    def set_rc(self, a, b, c, d):
        """设置遥控器的 4 个通道杆量
        a: 横滚 (-100~100)
        b: 俯仰 (-100~100)
        c: 油门 (-100~100)
        d: 偏航 (-100~100)"""
        self.connect.send(cmd='rc {} {} {} {}'.format(a, b, c, d))
        if output_mod:
            print(" *C* ", 'rc {} {} {} {}'.format(a, b, c, d))

    def set_wifi(self, wifi_id, wifi_password):
        """更改 Tello Wi-Fi 密码
        ssid: 更改后的 Wi-Fi 账号
        pass: 更改后的 Wi-Fi 密码
        如接入开源控制器，ssid 默认会加上 RMTT- 前缀，
        若未接则加上 TELLO- 前缀"""
        self.connect.send(cmd='wifi {} {}'.format(wifi_id, wifi_password))
        if output_mod:
            print(" *C* set:", 'wifi {} {}'.format(wifi_id, wifi_password))
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* wifi:", rec)
        return rec

    def pad_on(self):  # 打开挑战卡探测，默认打开下视探测
        self.connect.send(cmd='mon')
        if output_mod:
            print(" *C* set:", 'mon')
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* mon:", rec)
        return rec

    def pad_off(self):  # 关闭挑战卡探测
        self.connect.send(cmd='moff')
        if output_mod:
            print(" *C* set:", 'moff')
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* moff:", rec)
        return rec

    def mdirection(self, x):
        """X=0/1/2
        0 打开下视探测
        1 打开前视探测
        2 同时打开前视和下视探测
        *使用前必须使用 mon 命令打开探测功能，默认打
        开下视探测；
        * 单独打开前视或者下视探测时，探测频率为20Hz，同时打开前视和下视时，将交替探测，单个反向的探测频率为 10Hz。"""
        self.connect.send(cmd='mdirection {}'.format(x))
        if output_mod:
            print(" *C* set:", 'mdirection {}'.format(x))
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* mdirection:", rec)
        return rec

    def set_ap(self, wifi_id, wifi_password):
        """将 Tello 转为 station 模式，并连入到 AP
        ssid: 要连接的 Wi-Fi 账号
        pass: 要连接的 Wi-Fi 密码"""
        self.connect.send(cmd='ap {} {}'.format(wifi_id, wifi_password))
        if output_mod:
            print(" *C* set:", 'ap {} {}'.format(wifi_id, wifi_password))
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* ap:", rec)
        return rec

    def wifisetchannel(self, channel):
        """
        设置开源控制器 -WiFi 信道，
        xxx 为需要设置的信道。
        注意：清除信道设置需要清除 WiFi 信息，请根据各个地区政策设置符合法规的信道（仅开源控制器支持）"""
        self.connect.send(cmd='wifisetchannel {}'.format(channel))
        if output_mod:
            print(" *C* set:", 'wifisetchannel {}'.format(channel))
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* wifisetchannel:", rec)
        return rec

    def set_port(self, info, vedio):
        """设置状态信息和视频流推送端口，
        info 为状态信息推送端口，
        vedio 为视频信息推送端口，
        范围 :1025~65535"""
        self.connect.send(cmd='port {} {}'.format(info, vedio))
        if output_mod:
            print(" *C* set:", 'port {} {}'.format(info, vedio))
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* port:", rec)
        return rec

    def set_fps(self, fps):
        """
        设置视频流帧率，
        参数 fps 为帧率，
        值可以为high、middle、low，分别代表 30fps、15fps、5fps"""
        self.connect.send(cmd='setfps {}'.format(fps))
        if output_mod:
            print(" *C* set:", 'setfps {}'.format(fps))
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* setfps:", rec)
        return rec

    def set_bitrate(self, bitrate):
        """设置视频流比特率，参数 bitrate 为比特率，
        值可以为 0~5，分别代表自动、1Mbps、2MBps、3Mbps、4Mbps、5Mbps"""
        self.connect.send(cmd='setbitrate {}'.format(bitrate))
        if output_mod:
            print(" *C* set:", 'setbitrate {}'.format(bitrate))
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* setbitrate:", rec)
        return rec

    def set_resolution(self, resolution):
        """设置视频流分辨率，
        参数 resolution 为分辨率，
        值可以为 high 或 low，分别代表 720P、480P"""
        self.connect.send(cmd='setresolution {}'.format(resolution))
        if output_mod:
            print(" *C* set:", 'setresolution {}'.format(resolution))
        rec = self.connect.rec_result()
        if output_mod:
            print(" *C* setresolution:", rec)
        return rec

    def command(self):  # 进入SDK命令模式
        self.connect.send(cmd='command')
        if output_mod:
            print(" *C* ", 'command')

    def takeoff(self):  # 自动起飞
        self.connect.send(cmd='takeoff')
        if output_mod:
            print(" *C* ", 'takeoff')

    def land(self):  # 自动降落
        self.connect.send(cmd='land')
        if output_mod:
            print(" *C* ", 'land')

    def streamon(self):  # 打开视频流
        self.connect.send(cmd='streamon')
        if output_mod:
            print(" *C* ", 'streamon')

    def streamoff(self):  # 关闭视频流
        self.connect.send(cmd='streamoff')
        if output_mod:
            print(" *C* ", 'streamoff')

    def emergency(self):  # 停止电机转动
        self.connect.send(cmd='emergency')
        if output_mod:
            print(" *C* ", 'emergency')

    def motoron(self):  # 进入起桨模式（*注1）
        self.connect.send(cmd='motoron')
        if output_mod:
            print(" *C* ", 'motoron')

    def motoroff(self):  # 退出起桨模式
        self.connect.send(cmd='motoroff')
        if output_mod:
            print(" *C* ", 'motoroff')

    def throwfly(self):  # 抛飞，5s以内将飞机水平抛出，即可完成抛飞
        self.connect.send(cmd='throwfly')
        if output_mod:
            print(" *C* ", 'throwfly')

    def stop(self):  # 停止运动并悬停，任何时候都可以
        self.connect.send(cmd='stop')
        if output_mod:
            print(" *C* ", 'stop')

    def reboot(self):  # 重启飞行器
        self.connect.send(cmd='reboot')
        if output_mod:
            print(" *C* ", 'reboot')

    def up(self, x):  # 向上飞xcm
        self.connect.send(cmd='up {}'.format(x))
        if output_mod:
            print(" *C* ", 'up {}'.format(x))

    def down(self, x):  # 向下飞xcm
        self.connect.send(cmd='down {}'.format(x))
        if output_mod:
            print(" *C* ", 'down {}'.format(x))

    def left(self, x):  # 向左飞xcm
        self.connect.send(cmd='left {}'.format(x))
        if output_mod:
            print(" *C* ", 'left {}'.format(x))

    def right(self, x):  # 向右飞xcm
        self.connect.send(cmd='right {}'.format(x))
        if output_mod:
            print(" *C* ", 'right {}'.format(x))

    def forward(self, x):  # 向前飞x厘米
        self.connect.send(cmd='forward {}'.format(x))
        if output_mod:
            print(" *C* ", 'forward {}'.format(x))

    def back(self, x):  # 向后飞x厘米
        self.connect.send(cmd='back {}'.format(x))
        if output_mod:
            print(" *C* ", 'back {}'.format(x))

    def cw(self, x):  # 顺时针旋转x°
        self.connect.send(cmd='cw {}'.format(x))
        if output_mod:
            print(" *C* ", 'cw {}'.format(x))

    def ccw(self, x):  # 逆时针旋转x°
        self.connect.send(cmd='ccw {}'.format(x))
        if output_mod:
            print(" *C* ", 'ccw {}'.format(x))

    def turn_right(self, x):  # 顺时针旋转x°
        self.connect.send(cmd='cw {}'.format(x))
        if output_mod:
            print(" *C* ", 'turn_right {}'.format(x))

    def turn_left(self, x):  # 逆时针旋转x°
        self.connect.send(cmd='ccw {}'.format(x))
        if output_mod:
            print(" *C* ", 'turn_left {}'.format(x))

    def flip(self, x):  # 向x方向翻滚
        self.connect.send(cmd='flip {}'.format(x))
        if output_mod:
            print(" *C* ", 'flip {}'.format(x))

    def go(self, x, y, z, speed):  # 以自身坐标系直线飞行
        self.connect.send(cmd='go {} {} {} {}'.format(x, y, z, speed))
        if output_mod:
            print(" *C* ", 'go {} {} {} {}'.format(x, y, z, speed))

    def go_pad(self, x, y, z, speed, mid):  # 以挑战卡坐标系直线飞行
        self.connect.send(cmd='go {} {} {} {} {}'.format(x, y, z, speed, mid))
        if output_mod:
            print(" *C* ", 'go {} {} {} {} {}'.format(x, y, z, speed, mid))

    def curve(self, x1, y1, z1, x2, y2, z2, speed):  # 以自身坐标系弧线飞行
        self.connect.send(cmd='curve {} {} {} {} {} {} {}'.format(x1, y1, z1, x2, y2, z2, speed))
        if output_mod:
            print(" *C* ", 'curve {} {} {} {} {} {} {}'.format(x1, y1, z1, x2, y2, z2, speed))

    def curve_pad(self, x1, y1, z1, x2, y2, z2, speed, mid):  # 以挑战卡坐标系弧线飞行
        self.connect.send(cmd='curve {} {} {} {} {} {} {} {}'.format(x1, y1, z1, x2, y2, z2, speed, mid))
        if output_mod:
            print(" *C* ", 'curve {} {} {} {} {} {} {} {}'.format(x1, y1, z1, x2, y2, z2, speed, mid))

    def jump(self, x, y, z, speed, yaw, mid1, mid2):  # 以当前挑战卡坐标系飞往某点后悬停，识别第二张挑战卡，并在第二张挑战卡的正上方转动yaw°
        self.connect.send(cmd='curve {} {} {} {} {} {} {}'.format(x, y, z, speed, yaw, mid1, mid2))
        if output_mod:
            print(" *C* ", 'curve {} {} {} {} {} {} {}'.format(x, y, z, speed, yaw, mid1, mid2))
